package Pathall;

import Activemq.Node;
import Redis.Rdo;

import java.util.*;

public class BidirectionalAStarPlanner implements PathPlanner {
    // 方向枚举
    private enum Direction {
        UP(-1, 0),
        DOWN(1, 0),
        LEFT(0, -1),
        RIGHT(0, 1);

        public final int deltaX;
        public final int deltaY;

        Direction(int deltaX, int deltaY) {
            this.deltaX = deltaX;
            this.deltaY = deltaY;
        }
    }

    @Override
    public List<int[]> findPath(int[] start, int[] end, Rdo rdo, int mapWidth, int mapHeight) {
        if (start == null || end == null) return new ArrayList<>();
        if (!isValidCoordinate(end[0], end[1], mapWidth, mapHeight)) return new ArrayList<>();

        // 初始化起点和终点
        Node startNode = new Node(null, start[0], start[1]);
        Node goalNode = new Node(null, end[0], end[1]);
        if (startNode.equals(goalNode)) {
            return Collections.singletonList(new int[]{start[0], start[1]});
        }

        // 双向搜索数据结构
        PriorityQueue<Node> openListStart = new PriorityQueue<>();
        PriorityQueue<Node> openListGoal = new PriorityQueue<>();
        Map<String, Node> closedListStart = new HashMap<>();
        Map<String, Node> closedListGoal = new HashMap<>();
        Map<String, Node> nodeCacheStart = new HashMap<>();
        Map<String, Node> nodeCacheGoal = new HashMap<>();

        openListStart.add(startNode);
        openListGoal.add(goalNode);
        nodeCacheStart.put(startNode.getKey(), startNode);
        nodeCacheGoal.put(goalNode.getKey(), goalNode);

        Node meetingNode = null;
        Node meetingNodeOtherSide = null;

        while (!openListStart.isEmpty() && !openListGoal.isEmpty()) {
            // 从起点方向扩展
            Node currentNodeStart = openListStart.poll();
            closedListStart.put(currentNodeStart.getKey(), currentNodeStart);

            // 检查是否相遇
            if (closedListGoal.containsKey(currentNodeStart.getKey())) {
                meetingNode = currentNodeStart;
                meetingNodeOtherSide = closedListGoal.get(currentNodeStart.getKey());
                break;
            }
            // 关键修改：允许终点有障碍物
            expandNode(currentNodeStart, openListStart, closedListStart, nodeCacheStart,
                    true, goalNode, rdo, mapWidth, mapHeight);

            // 从终点方向扩展
            Node currentNodeGoal = openListGoal.poll();
            closedListGoal.put(currentNodeGoal.getKey(), currentNodeGoal);

            if (closedListStart.containsKey(currentNodeGoal.getKey())) {
                meetingNode = closedListStart.get(currentNodeGoal.getKey());
                meetingNodeOtherSide = currentNodeGoal;
                break;
            }
            // 关键修改：允许终点有障碍物
            expandNode(currentNodeGoal, openListGoal, closedListGoal, nodeCacheGoal,
                    false, startNode, rdo, mapWidth, mapHeight);
        }

        // 重建路径
        if (meetingNode != null && meetingNodeOtherSide != null) {
            return reconstructFullPath(meetingNode, meetingNodeOtherSide);
        }

        return new ArrayList<>();
    }

    // 扩展节点 - 关键修改：允许终点有障碍物
    private void expandNode(Node node, PriorityQueue<Node> openList, Map<String, Node> closedList,
                            Map<String, Node> nodeCache, boolean isForward, Node target,
                            Rdo rdo, int mapWidth, int mapHeight) {
        for (Direction dir : Direction.values()) {
            int neighborX = node.x + dir.deltaX;
            int neighborY = node.y + dir.deltaY;

            // 检查边界
            if (!isValidCoordinate(neighborX, neighborY, mapWidth, mapHeight)) {
                continue;
            }

            // 关键修改：允许目标点有障碍物
            boolean isTarget = (neighborX == target.x && neighborY == target.y);
            if (!isTarget && rdo.checkobstacle(neighborX, neighborY)) {
                continue;
            }

            String key = neighborX + "," + neighborY;
            if (closedList.containsKey(key)) continue;

            Node neighborNode = nodeCache.getOrDefault(key, new Node(null, neighborX, neighborY));
            double newG = node.g + 1;

            if (newG < neighborNode.g || !nodeCache.containsKey(key)) {
                neighborNode.g = newG;
                neighborNode.h = heuristic(neighborX, neighborY,
                        isForward ? target.x : node.x,
                        isForward ? target.y : node.y);
                neighborNode.f = neighborNode.g + neighborNode.h;
                neighborNode.parent = node;

                if (!nodeCache.containsKey(key)) {
                    openList.add(neighborNode);
                    nodeCache.put(key, neighborNode);
                } else {
                    // 更新队列中的优先级
                    openList.remove(neighborNode);
                    openList.add(neighborNode);
                }
            }
        }
    }

    // 带线性冲突检测的启发函数
    private double heuristic(int x1, int y1, int x2, int y2) {
        int dx = Math.abs(x1 - x2);
        int dy = Math.abs(y1 - y2);

        // 线性冲突检测（增加0.5的惩罚）
        if (x1 == x2 || y1 == y2) {
            return dx + dy;
        }
        return dx + dy + 0.5;
    }

    // 关键修复：确保路径连续无跳跃
    private List<int[]> reconstructFullPath(Node meetingNode, Node meetingNodeOtherSide) {
        // 重建起点到相遇点的路径
        List<int[]> pathFromStart = new ArrayList<>();
        Node current = meetingNode;
        while (current != null) {
            pathFromStart.add(new int[]{current.x, current.y});
            current = current.parent;
        }
        Collections.reverse(pathFromStart); // 从起点到相遇点

        // 重建终点到相遇点的路径
        List<int[]> pathFromGoal = new ArrayList<>();
        current = meetingNodeOtherSide;
        while (current != null) {
            pathFromGoal.add(new int[]{current.x, current.y});
            current = current.parent;
        }

        // 合并路径，确保连续
        List<int[]> fullPath = new ArrayList<>(pathFromStart);
        fullPath.addAll(pathFromGoal.subList(1, pathFromGoal.size()));

        // 验证路径连续性并修复
        return ensurePathContinuity(fullPath);
    }

    // 确保路径每一步都是相邻的
    private List<int[]> ensurePathContinuity(List<int[]> path) {
        if (path.isEmpty()) return path;

        List<int[]> continuousPath = new ArrayList<>();
        continuousPath.add(path.get(0));

        for (int i = 1; i < path.size(); i++) {
            int[] prev = continuousPath.get(continuousPath.size() - 1);
            int[] current = path.get(i);

            // 如果当前点与前一个点不相邻，添加中间点
            if (Math.abs(prev[0] - current[0]) + Math.abs(prev[1] - current[1]) > 1) {
                // 计算中间点
                int dx = Integer.signum(current[0] - prev[0]);
                int dy = Integer.signum(current[1] - prev[1]);

                int x = prev[0] + dx;
                int y = prev[1] + dy;

                // 沿直线方向添加所有中间点
                while (x != current[0] || y != current[1]) {
                    continuousPath.add(new int[]{x, y});
                    x += dx;
                    y += dy;
                }
            }

            continuousPath.add(current);
        }

        return continuousPath;
    }

    // 坐标有效性检查
    private boolean isValidCoordinate(int x, int y, int mapWidth, int mapHeight) {
        return x >= 0 && x < mapWidth && y >= 0 && y < mapHeight;
    }
}